/* 
* @Author: obsidian
* @Date:   2015-12-13 22:08:31
* @Last Modified by:   obsidian
* @Last Modified time: 2016-04-18 21:03:49
*/
#include "camera.h"
#include "driver/can/can.h"
#include "module/GPS/GPS.h"
#include "control/GPSpath/GPSpath.h"
#include "control/GPSpath/pointpath.h"
#include "driver/math/inc/math.h"
#include "math.h"

static void camera_calibrate(void);

struct Camera camera = 
{
        .init = Can1_Init,
        .offset = 0,
        .calibrate = camera_calibrate
};

static char flag = 0;

static void camera_calibrate()
{
    if(camera.status == 0){return;}
        static char flag3 = 0;
        // float distance      =   0;
        // float theta         =   0;
        // if(gps.front_point.y > 9480 && gps.front_point.y < 9963 && !flag)
        // {
        //         distance = get_point2point_length(path[gpspath.current_index].point,gps.front_point);
        //         if(gps.front_point.x > path[gpspath.current_index].point.x)
        //         {
        //                 distance = - distance;
        //         }  
        //         //distance -= camera.offset - gpspath.camera_center;
        //         theta    = distance /  sqrt(gps.front_point.y * gps.front_point.y + gps.front_point.x * gps.front_point.x);
        //         gps.base_radian += theta;
        //         rotate((float *)&gps.rear_point, theta);
        //         flag = 1;
        // }
    if(flag3 == 0 && gpspath.current_index > 1540 && gpspath.current_index < 1591 )
    {
        if(camera.offset == 0){return;}
        gps.rear_point.y = 6579.0 - (camera.offset-gpspath.camera_center) * gpspath.camera_k * sin(-gps.radian) - (gps.car_len + 120) * cos(-gps.radian);

        flag3 = 1;
    }

}
